#!/usr/bin/env python3

# 把全局规划器桥接到 move_base
# 未完成

import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped

class PlannerConnector:
    def __init__(self):
        rospy.init_node('planner_connector', anonymous=False)
        
        # 从参数服务器读取话题配置
        self.global_plan_topic = rospy.get_param('~gp_global_plan_topic', '/global_plan')
        self.nav_goal_topic = rospy.get_param('~gp_nav_goal_topic', '/nav_goal')
        self.map_frame_id = rospy.get_param('~gp_map_frame_id', 'map')
        
        # 创建订阅者
        self.path_sub = rospy.Subscriber(
            self.global_plan_topic, 
            Path, 
            self.path_callback,
            queue_size=1
        )
        self.goal_sub = rospy.Subscriber(
            self.nav_goal_topic, 
            PoseStamped, 
            self.goal_callback,
            queue_size=1
        )
        
        # 创建发布者
        self.move_base_plan_pub = rospy.Publisher(
            '/plan',  # move_base 默认订阅的全局路径话题
            Path, 
            queue_size=1
        )
        self.move_base_goal_pub = rospy.Publisher(
            '/move_base_simple/goal',  # RViz 目标发布标准话题
            PoseStamped, 
            queue_size=1
        )
        
        rospy.loginfo("PlannerConnector 已启动，正在桥接路径规划器")

    def path_callback(self, msg):
        """转发全局路径到 move_base"""
        try:
            # 确保坐标系一致
            if msg.header.frame_id != self.map_frame_id:
                rospy.logwarn(f"路径坐标系不匹配: {msg.header.frame_id} != {self.map_frame_id}")
                return
                
            self.move_base_plan_pub.publish(msg)
        except Exception as e:
            rospy.logerr(f"路径转发失败: {str(e)}")

    def goal_callback(self, msg):
        """转发导航目标到 move_base"""
        try:
            # 确保坐标系一致
            if msg.header.frame_id != self.map_frame_id:
                rospy.logwarn(f"目标坐标系不匹配: {msg.header.frame_id} != {self.map_frame_id}")
                return
                
            self.move_base_goal_pub.publish(msg)
        except Exception as e:
            rospy.logerr(f"目标转发失败: {str(e)}")

if __name__ == '__main__':
    try:
        connector = PlannerConnector()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass